#pragma once
#ifndef FRONTEND_H
#define FRONTEND_H

#include <opencv2/features2d.hpp>
#include "common.h"
#include "frame.h"
#include "map.h"

namespace myslam{
    
    class Backend;
    class Viewer;
    
    enum class FrontendStatus{ INITING,TRACKING_GOOD, TRACKING_BAD, LOST}；
    
    /***
     * 前端
     * 估计当前帧pose， 在满足关键帧条件时想地图加入关键帧并触发优化
     */
    class Frontend
    {
    public:
        EIGEN_MAKE_ALIGEND_OPERATOR_NEW;
        typedef std::shared_ptr<Frontend> Ptr;
         
        Frontend();
        
        ///外部接口，添加一个帧并计算起定位
        bool AddFrame(Frame::Ptr frame);
        
        ///Set函数
        void SetMap(Map::Ptr map){map_=map;}
        
        void SetBackend(std::shared_ptr<Backend> backend) {backend_=backend;}
        
        void SetViewer(std::shared_ptr<Viewer> viewer) {viewer_=viewer;}
        
        FrontendStatus GetStatus() const {return status_;}
        
        void SetCameras(Camera::Ptr left,Camera::Ptr right){
            camera_left_=left;
            camera_right_=right;
        }
        
    private:
        /***
         * Track in normal mode
         * @return true if success
         */
        bool Track();
        
        /**
         * Reset when lost
         * @return true if success
         */
        bool Reset();
        
        /***
         * Track with last frame
         *@return num of tracked points
         */
        int TrackLastFrame();
        
        /**
         * estimate current frame's pose
         * @return num of inliers
         */
        int EstimateCurrentPose();
        
        /**
         * set current frame as a keyframe and insert it into backend
         * @return true if success
         */
        bool InsertKeyframe();
        
        /**
         * Try init the frontend with stereo iamges saved in current_frame_
         * @return true if success
         */
        bool StereoInit();
        
        /**
         * Detect features in left image in current_frame_
         * keypoints will be saved in current_frame_
         * @return 
         */
        int DetectFaetures();
        
        /**
         * Find the corresponding faetures in right image of current_frame_
         * @return num of features found
         */
        int FindFeaturesInRight();
        
        /**
         * Build the initial map with single image
         * @return true if succeed
         */
        bool BuildInitMap();
        
        /***
         * Triangulate the 2D points in current farme
         *@return num of Triangulate points
         */
        int TriangulateNewPoints();
        
        /**
         * Set the features in keyframe as new observation of the map points
         */
        void SetObservationForKeyFrame();
        
        //data
        FrontendStaus status_=FrontendStatus::INITING;
        
        Frame::Ptr current_frame_=nullptr;  //当前帧
        Frame::Ptr last_frame_=nullptr;     //上一帧
        Camera::Ptr camera_left_=nullptr;   //左侧相机
        Camera::Ptr camera_right_=nullptr;  //右侧相机
        
        Map::Ptr map_=nullptr;
        std::shared_ptr<Backend> backend_=nullptr;
        
        std::shared_ptr<Viewer> viewer_=nullptr;
        
        SE3 relative_motion_;  //当前帧与上一帧的相对运动，用于估计当前帧的pose初值
        
        int tracking_inliers_=0; //inlies, used for testing new keyframes
        
        //params
        int num_features_=200;
        int num_features_init_=100;
        int num_features_tracking_=50;
        int num_features_tracking_bad=20；
        int num_faetures_needed_for_keyframe_=80;
        
        //utilities
        cv::Ptr<cv::GFTTDetector> gftt_;  //feature detector in opencv
    };
    
} //namespace myslam 

#endif // FRONTEND_H
